/*
 * @Description:  
 * @Author: ~光~~
 * @Date: 2024-08-25 10:29:02
 * @LastEditTime: 2024-10-11 11:19:19
 * @LastEditors:  
 */
/*
 * OB_GINS: An Optimization-Based GNSS/INS Integrated Navigation System
 *
 * Copyright (C) 2022 i2Nav Group, Wuhan University
 *
 *     Author : Hailiang Tang
 *    Contact : thl@whu.edu.cn
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <https://www.gnu.org/licenses/>.
 */

#ifndef IMUFILELOADER_H
#define IMUFILELOADER_H

#include "fileloader.h"
#include "src/common/types.h"


// ImuFileLoader 类：用于从文件中加载和解析 IMU 数据
class ImuFileLoader : public FileLoader {

public:
    // 构造函数被禁用，禁止无参数初始化
    ImuFileLoader() = delete;
    
    // 带参数的构造函数，用于打开 IMU 数据文件并初始化采样率
    ImuFileLoader(const string &filename, int columns, int rate = 200) {
        open(filename, columns, FileLoader::TEXT); // 打开指定文件

        dt_ = 1.0 / (double) rate; // 计算采样时间间隔

        imu_.time = 0; // 初始化 IMU 时间为 0
    }

    // 获取下一个 IMU 数据点并返回
    const IMU &next() {
        imu_pre_ = imu_; // 保存前一个 IMU 数据

        data_ = load(); // 从文件中加载新数据

        // 更新 IMU 数据结构中的时间戳、角速度和速度增量
        imu_.time = data_[0];
        memcpy(imu_.dtheta.data(), &data_[1], 3 * sizeof(double));
        memcpy(imu_.dvel.data(), &data_[4], 3 * sizeof(double));

        // 计算时间步长
        double dt = imu_.time - imu_pre_.time;
        if (dt < 0.1) {
            imu_.dt = dt; // 使用实际时间差
        } else {
            imu_.dt = dt_; // 使用默认时间间隔
        }

        // 根据列数来处理增量速度
        if (columns_ == 8) {
            imu_.odovel = data_[7] * imu_.dt;
        } else if (columns_ == 9) {
            imu_.odovel = 0.5 * (data_[7] + data_[8]) * imu_.dt;
        }

        return imu_; // 返回当前 IMU 数据
    }

private:
    double dt_; // IMU 数据的默认时间间隔

    IMU imu_, imu_pre_; // 当前和前一个 IMU 数据
    vector<double> data_; // 从文件中加载的原始数据
};

#endif // IMUFILELOADER_H

